//      optimization.h
//      Optimization algorithm 
//      
//      Copyright 2012 tqlong <tqlong@espada>
//      
//      This program is free software; you can redistribute it and/or modify
//      it under the terms of the GNU General Public License as published by
//      the Free Software Foundation; either version 2 of the License, or
//      (at your option) any later version.
//      
//      This program is distributed in the hope that it will be useful,
//      but WITHOUT ANY WARRANTY; without even the implied warranty of
//      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//      GNU General Public License for more details.
//      
//      You should have received a copy of the GNU General Public License
//      along with this program; if not, write to the Free Software
//      Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
//      MA 02110-1301, USA.

#ifndef __OMPTIMIZATION_H
#define __OMPTIMIZATION_H

#include "ublas_helper.h"

template <typename Func>
struct LineSearch{
  typedef typename Func::var_t var_t;
  typedef typename Func::helper_t helper_t;

  Func& f_;
  double alpha_;
  var_t x_;
  
  LineSearch(Func& f) : f_(f), alpha_(1.0), x_(f.init_x0_) {}

  void search(const var_t& x0, const var_t& dir, double f0, double dot_dir_grad) {
    if (dot_dir_grad >= 0) {
      MSG("Not a descending direction");
      return;
    }

    double c1 = 1e-4;
    x_ = x0.add(dir, alpha_);
    helper_t h;

    f_.helper(x_, h);
    double f_val = f_(x_, h);
    
    while (f_val > f0 + c1 * alpha_ * dot_dir_grad && alpha_ > 1e-16) {
      alpha_ *= 0.1;
      x_ = x0.add(dir, alpha_);
      f_.helper(x_, h);
      f_val = f_(x_, h);
    }
  }
};

template <typename Func>
struct GradientDescent{
  typedef typename Func::var_t var_t;
  typedef typename Func::helper_t helper_t;

  Func& f_;
  var_t x_;
  int maxiter_;
  
  GradientDescent(Func& f, int maxiter) : f_(f), x_(f.init_x0_), maxiter_(maxiter) {}

  void search(const var_t& x0) {
    helper_t h; 
    var_t grad = f_.init_x0_;
    x_ = x0;
    
    f_.helper(x_, h);    
    double f_val = f_(x_, h);
    f_.grad(x_, h, grad);

    for (int iter = 1; iter <= maxiter_; iter++) {
      MSG("iter=" << iter << " f=" << f_val << " norm(grad)=" << grad.norm());
      
      LineSearch<Func> line(f_);
      var_t dir = grad.reverse();
      double dot_dir_grad = dir.dot(grad);
      line.search(x_, dir, f_val, dot_dir_grad);

      x_ = line.x_;
      f_.helper(x_, h);
      f_val = f_(x_, h);
      f_.grad(x_, h, grad);
    }
  }
};

#endif
